#include "widget.h"

//设置为自己电脑上机械臂的端口号
#define Port "COM5"

//棋盘（0,0）对应的空间坐标系内坐标
#define ChessX_0 1000
#define ChessY_0 2000
#define ChessZ_0 50

//棋间距
#define ChessSize 40

Widget::Widget(QWidget *parent)
    : QWidget(parent)
{
    //连接COM13，波特率115200
    connectPort();
}

Widget::~Widget()
{
    if (serial->isOpen()) {
        serial->close();
        qDebug() << "Serial port closed";
    }
}
void Widget::sendCommand(const QString &cmd, const QString &action)
{
    if (serial->isOpen()) {
        serial->write(cmd.toUtf8());
        qDebug()<<"状态：" + action + "命令已发送";
        qDebug() << action << "命令已发送：" << cmd;
    } else {
        QMessageBox::warning(this, "错误", "串口未打开，无法发送命令");
        qDebug()<<"状态：串口未打开";
        qDebug() << "Failed to send command, serial port not open";
    }
}
void Widget::connectPort(void)
{
    // 创建串口对象
    serial = new QSerialPort(this);
    // 打开串口
    serial->setPortName(Port);  // 替换为实际串口号
    serial->setBaudRate(QSerialPort::Baud115200);
    if (!serial->open(QIODevice::ReadWrite)) {
        QMessageBox::warning(this, "错误", "无法打开串口！请检查连接");
        qDebug()<<"状态：无法打开串口";
        qDebug() << "Failed to open port";
    } else {
        qDebug()<<"状态：串口已打开";
        qDebug() << "Serial port opened successfully";
    }
}
void Widget::kinematics_move(float x, float y, float z)
{
    //int i,j, min = 0, flag = 0;
    int i, min = 0, flag = 0;
    if(y < -80)return ;
    //寻找最佳角度
    flag = 0;
    for(i=-0;i>=-135;i--)
    {
       if(0 == kinematics_analysis(x,y,z,i))
       {
          if(i<min)min = i;
           flag = 1;
       }
    }

    //用3号舵机与水平最大的夹角作为最佳值
    if(flag)
    {
        kinematics_analysis(x,y,z,min);
        return ;
    }
    return ;
}
int Widget::kinematics_analysis(float x, float y, float z, float Alpha)
{
    float theta3, theta4, theta5, theta6;
    float aaa, bbb,ccc, zf_flag;
    float pi = 3.1415926;

    float servo_angle[4];

    float l0, l1, l2, l3;//L0 L1  L2  L3
    l0 = 900;
    l1 = 1050;
    l2 = 980;
    l3 = 1600;

    //放大10倍
    x = x*10;
    y = y*10;
    z = z*10;

    //优化后机械臂可以在-y轴活动，即活动范围大于180度
         if(x == 0){
            theta6 = 0.0;
         }
        else if(x>0 && y<0){
            theta6 = atan(x / y);
            theta6 = 180 + (theta6 * 180.0/pi);
        }else{
                  if(y>=0){
                       theta6 = atan(x / y);
               theta6 = theta6 * 180.0 / pi;
                    }else{

                    theta6 = atan(x / y);
              theta6 = theta6 * 180.0 / pi;
                      theta6=theta6-180;
                    }

            }
          y = sqrt(x*x + y*y);//#  x,y坐标的斜边
        y = y-l3 * cos(Alpha*pi/180.0);  //求出 y总 - y3 = y2 + y1
        z = z-l0-l3*sin(Alpha*pi/180.0); //求出z1 + z2
        if(z < -l0) {
            return 1;
        }
        if(sqrt(y*y + z*z) > (l1+l2)) {
            return 2;
        }

        ccc = acos(y / sqrt(y * y + z * z));
        bbb = (y*y+z*z+l1*l1-l2*l2)/(2*l1*sqrt(y*y+z*z));//余弦定理
        if(bbb > 1 || bbb < -1) {
            return 5;
        }
        if (z < 0) {
            zf_flag = -1;
        } else {
            zf_flag = 1;
        }
        theta5 = ccc * zf_flag + acos(bbb);
        theta5 = theta5 * 180.0 / pi;
        if(theta5 > 180.0 || theta5 < 0.0) {
            return 6;
        }

        aaa = -(y*y+z*z-l1*l1-l2*l2)/(2*l1*l2);
        if (aaa > 1 || aaa < -1) {
            return 3;
        }
        theta4 = acos(aaa);
        theta4 = 180.0 - theta4 * 180.0 / pi ;
        if (theta4 > 135.0 || theta4 < -135.0) {
            return 4;
        }

        theta3 = Alpha - theta5 + theta4;
        if(theta3 > 90.0 || theta3 < -90.0) {
            return 7;
        }

        servo_angle[0] = theta6;
        servo_angle[1] = theta5-90;
        servo_angle[2] = theta4;
        servo_angle[3] = theta3;

        //换算舵机角度
        servo_pwm[0] = (int)(1500-2000.0 * servo_angle[0] / 270.0);
        servo_pwm[1] = (int)(1500+2000.0 * servo_angle[1] / 270.0);
        servo_pwm[2] = (int)(1500+2000.0 * servo_angle[2] / 270.0);
        servo_pwm[3] = (int)(1500-2000.0 * servo_angle[3] / 270.0);

        for(int i = 0; i<4;i++)
        {
            //qDebug()<<"servo_angle"<<i<<" "<<servo_angle[i];
            //qDebug()<<"servo_pwm"<<i<<" "<<servo_pwm[i];
        }

        return 0;
}
void Widget::actionGroupPut(int x,int y,int z,int time)
{
    const int Z_OFFSET = 60;
    const int CLAW_OPEN_PWM = 1000;
    const int CLAW_CLOSE_PWM = 1600;

    // 移动到 (x, y, z + Z_OFFSET)
    kinematics_move(x, y, z + Z_OFFSET);
    //kinematics_move(xSpinBox->value(),ySpinBox->value(),zSpinBox->value()+60);

    // 构建命令字符串
    QString command = buildCommand(servo_pwm, time, CLAW_CLOSE_PWM);
    sendCommand(command, "放置:移动到上方");


    // 移动到目标位置 (x, y, z)
    kinematics_move(x, y, z);
    //kinematics_move(xSpinBox->value(),ySpinBox->value(),zSpinBox->value());

    // 构建命令字符串
    command = buildCommand(servo_pwm, time, CLAW_CLOSE_PWM);

    // 设置一个单次定时器延时 time 毫秒后执行
    QTimer::singleShot(time, this, [this, command]() {
        sendCommand(command, "放置:移动最佳位置");
    });

    commandClaw = QString("#005P%1T%2!").arg(CLAW_OPEN_PWM).arg(time, 3, 10, QChar('0'));

    // 设置一个单次定时器延时 2 * time 毫秒后打开爪子
    QTimer::singleShot(2 * time, this, [this]() {
        sendCommand(commandClaw, "放置:爪子打开");
    });

}
void Widget::actionGroupGet(int x, int y, int z, int time)
{
    const int Z_OFFSET = 60;
    const int CLAW_OPEN_PWM = 1000;
    const int CLAW_CLOSE_PWM = 1600;

    // 移动到 (x, y, z + Z_OFFSET)
    kinematics_move(x, y, z + Z_OFFSET);
    //kinematics_move(xSpinBox->value(),ySpinBox->value(),zSpinBox->value()+60);

    // 构建命令字符串
    QString command = buildCommand(servo_pwm, time, CLAW_OPEN_PWM);
    sendCommand(command, "抓取:移动到上方开爪子");

    // 移动到目标位置 (x, y, z)
    kinematics_move(x, y, z);
    //kinematics_move(xSpinBox->value(),ySpinBox->value(),zSpinBox->value());

    // 构建命令字符串
    command = buildCommand(servo_pwm, time, CLAW_OPEN_PWM);

    // 设置一个单次定时器延时 time 毫秒后执行
    QTimer::singleShot(time, this, [this, command]() {
        sendCommand(command, "抓取:移动最佳位置");
    });

    commandClaw = QString("#005P%1T%2!").arg(CLAW_CLOSE_PWM).arg(time, 3, 10, QChar('0'));

    // 设置一个单次定时器延时 2 * time 毫秒后关闭爪子
    QTimer::singleShot(2 * time, this, [this]() {
        sendCommand(commandClaw, "抓取:爪子关闭");
    });
}

QString Widget::buildCommand(const int servo_pwm[], int time, int claw_pwm)
{
    QString command = "{#000P%1T%2!#001P%3T%4!#002P%5T%6!#003P%7T%8!#005P%9T%10!}";
    return command.arg(servo_pwm[0], 3, 10, QChar('0')).arg(time, 3, 10, QChar('0'))
           .arg(servo_pwm[1], 3, 10, QChar('0')).arg(time, 3, 10, QChar('0'))
           .arg(servo_pwm[2], 3, 10, QChar('0')).arg(time, 3, 10, QChar('0'))
           .arg(servo_pwm[3], 3, 10, QChar('0')).arg(time, 3, 10, QChar('0'))
           .arg(claw_pwm).arg(time, 3, 10, QChar('0'));
}
void Widget::BYxyPut(int ChessX,int ChessY,int time)
//通过xy放置:该函数通过给出的棋盘坐标（x,y）计算出空间坐标进行抓取
{
//    //棋盘（0,0）对应的空间坐标系内坐标
//    #define ChessX_0 1000
//    #define ChessY_0 2000
//    #define ChessZ_0 50

//    //棋间距
//    #define ChessSize 40

//    //空间坐标系的点（x,y,z）

    //（ChessX,ChessY）ChessX每＋1，X-40;ChessY每加1，y-40;
    int x = ChessX_0-(ChessSize*ChessX);
    int y = ChessY_0-(ChessSize*ChessY);

    actionGroupPut(x,y,ChessZ_0,time);
}
void Widget::BYxyPutPro(int ChessX,int ChessY,int time)
//通过xy放置:该函数通过给出的棋盘坐标（x,y）调用给死的9个空间坐标进行抓取
{
    //定义一个三维数组，保存，9个格子的坐标
    int arry[3][3][3] = {{{0}}};

    // 初始化9个格子的坐标
    arry[0][0][0] = 257;
    arry[0][0][1] = 32;
    arry[0][0][2] = 40;

    arry[0][1][0] = 255;
    arry[0][1][1] = -15;
    arry[0][1][2] = 40;

    arry[0][2][0] = 250;
    arry[0][2][1] = -60;
    arry[0][2][2] = 40;

    //-----

    arry[1][0][0] = 212;
    arry[1][0][1] = 35;
    arry[1][0][2] = 35;

    arry[1][1][0] = 210;
    arry[1][1][1] = -12;
    arry[1][1][2] = 35;

    arry[1][2][0] = 205;
    arry[1][2][1] = -62;
    arry[1][2][2] = 35;

    //----

    arry[2][0][0] = 168;
    arry[2][0][1] = 38;
    arry[2][0][2] = 33;

    arry[2][1][0] = 162;
    arry[2][1][1] = -10;
    arry[2][1][2] = 33;

    arry[2][2][0] = 160;
    arry[2][2][1] = -58;
    arry[2][2][2] = 33;



//    // 输出数组元素，以便检查结果
//    for (int i = 0; i < 2; ++i) {
//        for (int j = 0; j < 2; ++j) {
//            for (int k = 0; k < 2; ++k) {
//                qDebug() << "arry[" << i << "][" << j << "][" << k << "] = " << arry[i][j][k];
//            }
//        }
//    }


    //调用抓取函数
    actionGroupPut( arry[ChessX][ChessY][0],
                    arry[ChessX][ChessY][1],
                    arry[ChessX][ChessY][2],
            time);
}
